% Converts the point in [x,y,z] system into angles [A,B,C] by using
% the known physical dimensions of the arm.
%
%             /\
%            /  \
%           / B  \
%          /      \ FEMUR [45.45 mm]
%         /        \
%  TIBIA /          \
% [59.3 mm]       A1 \__________Gamma (rotate)
%      /          . '|    COXA [14.8 mm]
%     /       . ' A0
%    /     . '       |
%   /  . '            HEIGTH [92 mm]    ^
%  /. '              |                   \
% -X- - - - - - - - - - - - - - - x       \ y
%  SP[51,51,51]                            \
%
% Angle Gamma is the rotation of the whole arm around the shoulder
% pivot.
%
%	j = hypot(x,y)-coxa;
%
%	G=atan(y/x);
%	A0=atan(j/z);
%	A1=acos((femur^2+j^2+z^2-tibia^2)/(2*hypot(j,z)*femur));
%	B=acos((tibia^2+femur^2-z^2-j^2)/(2*femur*tibia));
%
function ret = p2a( robot, P )
coxa = robot(1,1);%coxa=14.8;
femur= robot(1,2);%femur=45.45; 
tibia= robot(1,3);%tibia=59.29; 
base = robot(1,4);%base=92;     
G_shift = 0;
signum = 1;

	x=P(1,1);
	y=P(1,2);
	z=base-P(1,3);

	% Determine the quadrant and derive the shift in gama 
	if(( y < 0 ) && ( x > 0 ))
		y = abs(y);
		G_shift = 2*pi;	
		signum = -1;
	endif

	if(( y > 0 ) && ( x < 0 ))
		x = abs(x);
		G_shift = pi;	
		signum = -1;
	endif

	if(( y < 0 ) && ( x < 0 ))
		x = abs(x);
		y = abs(y);
		G_shift = pi;	
	endif

	j = hypot(x,y)-coxa;

	G=atan(y/x);
	A0=atan(j/z);
	A1=acos((femur^2+j^2+z^2-tibia^2)/(2*hypot(j,z)*femur));
	B=acos((tibia^2+femur^2-z^2-j^2)/(2*femur*tibia));

	if( G_shift > 0 )
		G = G_shift + ( signum * G );
	endif

	% return both radians and degrees for convenience ...
	Gdeg=G*180/pi;
	Adeg=(A0+A1)*180/pi;
	Bdeg=B*180/pi;

	ret(1,1)=Gdeg;
	ret(1,2)=Adeg;
	ret(1,3)=Bdeg;
	ret(2,1)=G;
	ret(2,2)=(A0+A1);
	ret(2,3)=B;
endfunction
